#include "S_CarTask.h"
#include "stdlib.h"
#include "math.h"

#define CARL 0.057
static float step = 0.008f;

uint16_t __discount = 0;

extern PID_t Ang;
extern PID_t pos1;
extern PID_t pos2;
extern float yaw;
extern float Lwheelpos;
extern float Rwheelpos;
extern char Rx1Buffer[RXBUFFERSIZE];
extern uint8_t Uart1RXFlag;

static uint8_t isfinished(PID_t *pid, uint8_t type)
{
	static uint8_t isfinCNT = 0;
	if (type == 0)
	{
		if (fabs(pid->current - pid->target) < 0.05)
			isfinCNT++;
		else
			isfinCNT = 0;
	}
	else if (type == 1 || type == 2)
	{
		if (fabs(yaw_calc_plus(pid->current, -pid->target)) < 0.1)
			isfinCNT++;
		else
			isfinCNT = 0;
	}
	if (isfinCNT > 10)
	{
		isfinCNT = 0;
		return 1;
	}
	else
		return 0;
}

void FreeMessionList(Mession_t *list)
{
	Mession_t *p;
	p = list->next;
	while (list->next != NULL)
	{
		free(list);
		list = p;
		p = list->next;
	}
	free(list);
}

void ShowMeMessionList(Mession_t *head)
{
	Mession_t *p = head;
	while (p->next != NULL)
	{
		p = p->next;
		if (p->type == STRAIGHT)
			UART_printf(&huart1, "type:s,dis:%.1f,isdoing:%d,isdone:%d,r:%d\n", p->distance, p->isdoing, p->isdone, p->redo);
		else if (p->type == TURN)
			UART_printf(&huart1, "type:t,Ang:%.1f,isdoing:%d,isdone:%d,r:%d\n", p->angle, p->isdoing, p->isdone, p->redo);
		else if (p->type == ROUND)
			UART_printf(&huart1, "type:r,rad:%.1f,theta:%.1f,isdoing:%d,isdone:%d,r:%d\n", p->radius, p->theta, p->isdone, p->isdoing, p->redo);
	}
}

Mession_t *CreateMessionListFromArray(Mession_t *array, uint8_t len)
{
	Mession_t *head, *node, *end; //头节点，普通节点和尾节点

	head = (Mession_t *)malloc(sizeof(Mession_t));
	end = head;

	for (uint8_t i = 0; i < len; i++)
	{
		node = (Mession_t *)malloc(sizeof(Mession_t));

		node->isdoing = 0;
		node->isdone = 0;
		node->type = array[i].type;
		if (array[i].type == STRAIGHT)
		{
			node->distance = array[i].distance;
			node->angle = 0;
			node->radius = 0;
			node->theta = 0;
		}
		else if (array[i].type == TURN)
		{
			node->distance = 0;
			node->angle = array[i].angle;
			node->radius = 0;
			node->theta = 0;
		}
		else if (array[i].type == ROUND)
		{
			node->distance = 0;
			node->angle = 0;
			node->radius = array[i].radius;
			node->theta = array[i].theta;
		}
		node->redo = array[i].redo;

		end->next = node;
		end = node;
	}
	end->next = NULL;
	return head;
}

Mession_t *CreateMessionListFromStr(char *str)
{
	Mession_t *head, *node, *end,*p;
	char *token = NULL;
	const char *delim = ",";
	int16_t redo=1, matchcnt;
	float dis, ang, rad, the;
	token = strtok(str, delim);
	if (strlen(token) >= 3)
	{
		head = (Mession_t *)malloc(sizeof(Mession_t));
		end = head;
	}
	else
		return NULL;
	while (token)
	{

		if (token[0] == 'S') //直走
		{
			matchcnt = sscanf(token, "S_%f", &dis);
			if (matchcnt != 1)
				return NULL;
			node = (Mession_t *)malloc(sizeof(Mession_t));
			node->isdoing = 0;
			node->isdone = 0;
			node->redo = 1;
			node->type = STRAIGHT;
			node->distance = dis/40.0f;
			node->angle = 0;
			node->radius = 0;
			node->theta = 0;
		}
		else if (token[0] == 'T') //转向
		{
			matchcnt = sscanf(token, "T_%f", &ang);
			if (matchcnt != 1)
				return NULL;
			node = (Mession_t *)malloc(sizeof(Mession_t));
			node->isdoing = 0;
			node->isdone = 0;
			node->redo = 1;
			node->type = TURN;
			node->distance = 0;
			node->angle = ang;
			node->radius = 0;
			node->theta = 0;
		}
		else if (token[0] == 'R') //画弧
		{
			matchcnt = sscanf(token, "R_r%ft%f", &rad, &the);
			if (matchcnt != 2)
				return NULL;
			node = (Mession_t *)malloc(sizeof(Mession_t));
			node->isdoing = 0;
			node->isdone = 0;
			node->redo = 1;
			node->type = ROUND;
			node->distance = 0;
			node->angle = 0;
			node->radius = rad;
			node->theta = the;
		}
		else if (token[0] == 'r')
		{
			sscanf(token,"r_%d",&redo);
		}
		else
			return NULL;
		end->next = node;
		end = node;
		token = strtok(NULL, delim);
	}
	end->next = NULL;

	p = head;
	while(p->next!=NULL)
	{
		p = p->next;
		p->redo = redo;
	}

	return head;
}

Mession_t *CreateMessionListFromUART(void)
{
	Mession_t *p;
	if (!Uart1RXFlag)
		return NULL;
	else
	{
		UART_printf(&huart1, Rx1Buffer);

		p = CreateMessionListFromStr(Rx1Buffer);
		Uart1RXFlag = 0;
		memset(Rx1Buffer, 0x00, sizeof(Rx1Buffer));
		return p;
	}
}

/**
 * @brief 静态函数用于处理走直线
 * 
 * @param node 
 * @param discount 
 */
static void StraightTask(Mession_t *node)
{
	Ang.current = yaw;
	PID_Calc_yaw(&Ang);
	Lwheelpos += Ang.OUT;
	Rwheelpos -= Ang.OUT;

	if (__discount)
	{
		if (node->distance > 0)
		{
			Lwheelpos += step;
			Rwheelpos += step;
		}
		else
		{
			Lwheelpos -= step;
			Rwheelpos -= step;
		}
		__discount--;
	}
	MOTORPosCtrl(Lwheelpos, Rwheelpos);
	if (!__discount)
		if (isfinished(&pos1, STRAIGHT))
		{
			node->isdoing = 0;
			node->isdone = 1;
			node->redo--;
		}
}

static void turnTask(Mession_t *node)
{
	Ang.current = yaw;
	PID_Calc_yaw(&Ang);
	Lwheelpos += Ang.OUT;
	Rwheelpos -= Ang.OUT;

	MOTORPosCtrl(Lwheelpos, Rwheelpos);

	if (isfinished(&Ang, TURN))
	{
		node->isdoing = 0;
		node->isdone = 1;
		node->redo--;
	}
}

static void roundTask(Mession_t *node)
{

	Ang.current = yaw;
	if (__discount)
	{
		if (node->theta > 0)
		{
			Ang.target = yaw_calc_plus(Ang.target, +0.01f * 180.0f * 0.159f / (node->radius * 3.14159f));
			PID_Calc_yaw(&Ang);
			Lwheelpos += Ang.OUT;
			Rwheelpos -= Ang.OUT;
			Lwheelpos += step * (node->radius + CARL) / node->radius;
			Rwheelpos += step * (node->radius - CARL) / node->radius;
		}
		else
		{
			Ang.target = yaw_calc_plus(Ang.target, -0.01f * 180.0f * 0.159f / (node->radius * 3.14159f));
			PID_Calc_yaw(&Ang);
			Lwheelpos += Ang.OUT;
			Rwheelpos -= Ang.OUT;
			Lwheelpos += step * (node->radius - CARL) / node->radius;
			Rwheelpos += step * (node->radius + CARL) / node->radius;
		}
		__discount--;
	}
	else
	{
		Lwheelpos = -pos1.current;
		Rwheelpos = -pos2.current;
	}
	MOTORPosCtrl(Lwheelpos, Rwheelpos);

	UART_printf(&huart1, "%d,%f,%f,%f,%f\n", __discount, Ang.target, Ang.current, pos1.current, Lwheelpos);
	if (!__discount)
		if (isfinished(&pos1, STRAIGHT))
		{
			node->isdoing = 0;
			node->isdone = 1;
			node->redo--;
		}
}

uint8_t RunTask(Mession_t *head)
{

	Mession_t *p = head;

	while (p->next != NULL)
	{
		p = p->next;
		if (p->isdone) //如果当前任务已完成则进入下一个节点
			continue;
		else if (p->redo == 0) //如果当前节点的运行次数已经为零则进入下一个节点
			continue;
		else
		{
			if (!p->isdoing)
			{
				p->isdoing = 1;
				Ang.target = yaw;
				Ang.target = yaw_calc_plus(yaw, p->angle);
				if (p->type == STRAIGHT)
					__discount = (uint16_t)(fabs(p->distance) / step);
				if (p->type == ROUND)
					__discount = (uint16_t)(fabs((p->radius * p->theta * 3.1415926f / 180.0f) / 0.159f) * 100.0f);
			}
			else
			{
				switch (p->type)
				{
				case STRAIGHT:
					StraightTask(p);
					break;
				case TURN:
					turnTask(p);
					break;
				case ROUND:
					roundTask(p);
					// UART_printf(&huart1, "round!\n");
					break;
				}
			}
			break;
		}
	}

	if (p->next == NULL && p->isdone && (p->redo != 0)) //当此时是链表尾并且已经完成
	{
		UART_printf(&huart1, "res!\n");
		while (head->next != NULL)
		{
			head = head->next;
			head->isdone = 0;
		}
	}

	if ((p->next == NULL) && p->isdone && (p->redo == 0))
		return 1;
	else
		return 0;
}